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ABSTRACT 

This  report  presents  algorithms  that  can  be  utilized  in  a  GPS  receiver  to  convert  satellite- 
to-receiver  pseudo-ranges  to  receiver  position  estimates.  The  report  discusses  a  method 
that  is  used  to  determine  instantaneous  estimates  of  receiver  position  and  then  goes  on  to 
develop  three  Kalman  filter  based  estimators,  which  use  stationary  receiver,  low  dynamics, 
and  high  dynamics  models  for  the  receiver  motion,  respectively.  These  particular  dynamic 
models  are  utilized  because  they  are  commonly  used  in  actual  GPS  receivers,  and  cover 
a  wide  range  of  applications.  While  the  standard  form  of  the  Kalman  filter,  of  which  the 
three  filters  just  mentioned  are  examples,  is  theoretically  correct,  it  can  be  susceptible 
to  numerical  round-off  errors,  which  can  in  some  cases  result  in  poor  performance  or,  in 
the  worst  case,  filter  divergence.  This  issue,  and  its  solution,  is  investigated,  and  another 
version  of  the  high  dynamics  filter,  which  addresses  this  problem,  is  presented.  Matlab 
code  was  developed  to  test  the  performance  of  each  of  the  filters  and  simulations  were 
performed.  The  results  of  the  simulations  are  also  presented. 

RELEASE  LIMITATION 

Approved  for  Public  Release 


UNCLASSIFIED 


UNCLASSIFIED 


Published  by 

Cyber  and  Electronic  Warfare  Division 
Defence  Science  and  Technology  Group 
PO  Box  1500 

Edinburgh,  South  Australia  5111,  Australia 

Telephone:  1300  333  362 

Facsimile:  (08)  7 389  6567 

(5)  Commonwealth  of  Australia  2016 
AR- 01 6-  601 
June,  2016 


APPROVED  FOR  PUBLIC  RELEASE 


UNCLASSIFIED 


UNCLASSIFIED 


Development  of  GPS  Receiver  Kalman  Filter 
Algorithms  for  Stationary,  Low-Dynamics,  and 
High-Dynamics  Applications 


Executive  Summary 

The  Global  Positioning  system  (GPS)  is  the  primary  source  of  information  for  a  broad 
range  of  positioning,  navigation  and  timing  systems.  It  is  an  all-weather,  satellite-based 
radio-navigation  system  which  provides  world-wide  coverage.  The  objective  of  this  report 
is  to  present  algorithms  used  in  a  central  component  of  the  system’s  receiver  position 
calculation,  viz.,  to  convert  the  satellite-to-receiver  pseudo-ranges  to  receiver  position  es¬ 
timates.  This  report  is  one  outcome  of  recent  efforts  to  expand  our  knowledge  base  for  this 
important  component  of  GPS  receiver  technology;  this  increased  knowledge  will  facilitate 
our  capabilities  to  provide  scientifically  based  advice  to  the  Australian  Defence  Force. 

The  report  first  describes  a  method  for  determining  instantaneous  estimates  of  receiver 
position,  and  then  goes  on  to  develop  three  Kalman  filter  based  receiver  position  estima¬ 
tors,  i.e.,  a  stationary  receiver,  low  dynamics,  and  high  dynamics  estimator.  As  is  implied 
by  their  names,  the  three  types  of  filters  incorporate  dynamic  models  that  are  optimized 
for  situations  where  the  receiver  is  stationary,  is  subjected  to  small  accelerations,  and  to 
large  accelerations,  respectively.  These  estimators  are  designed  to  optimize  performance 
for  commonly  occurring  applications,  as  is  done  in  many  GPS  receivers. 

The  development  of  the  three  types  of  Kalman  filter,  as  well  as  the  instantaneous  estimator 
is  presented  in  Section  2.  Section  3  then  presents  the  results  of  testing  by  simulation.  It 
is  found  that  the  simulations  give  indications  of  performance  degradation,  resulting  from 
errors  associated  with  numerical  round-off,  in  the  case  of  the  high  dynamics  Kalman  filter. 
This  is  then  further  investigated  in  Section  4  and  an  alternate  form  of  the  high  dynamics 
filter  is  then  developed  to  overcome  the  problem.  The  filter  was  implemented  in  Matlab 
and  tested  by  simulation.  The  results  of  the  simulations  are  also  in  Section  4.  Finally, 
concluding  remarks  are  presented  in  Section  5. 

While  this  report  deals  specifically  with  GPS  algorithms,  the  work  covered  forms  part 
of  a  larger  effort  to  develop  algorithms  for  fusing  GPS  measurements  with  other  sensor 
data,  particularly  measurements  from  inertial  navigation  systems  (INS),  to  support  R&D 
into  multisensor  positioning  and  navigation  performance  in  non-benign  environments.  The 
algorithms  presented  in  this  report,  and  software  developed  for  this  work,  will  be  required 
for  future  research  into  deep  integration  of  GPS  with  other  sources  of  position  data.  The 
software  will  also  be  useful  as  a  component  of  future  modelling  software  that  may  need  to 
be  developed  for  performance  prediction  of  current  or  future  systems  that  incorporate  GPS. 
The  ultimate  aim  is  to  help  inform  future  capability  requirements  through  the  outcomes 
of  this  research. 
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1  Introduction 


The  Global  Positioning  System  (GPS)  is  a  satellite-based  radio-navigation  system  that 
provides  world-wide,  all-weather,  coverage.  GPS  receivers  decode  and  process  messages 
from  in-view  satellites  to  determine  the  receiver’s  location  as  well  as  the  current  time;  in 
this  report,  GPS  receiver  position  determination  is  considered.  To  determine  the  receiver’s 
location,  the  GPS  system  uses  time  of  arrival  ranging.  Each  GPS  receiver  contains  an  in¬ 
ternal  clock  which  it  uses  to  determine  the  time  of  arrival  of  satellite  ranging  signals;  using 
this  information,  the  receiver  calculates  the  time  taken  for  the  signal  to  travel  from  the 
satellite  to  the  receiver.  Since  the  signal  travels  at  the  speed  of  light,  c;  this  time  interval 
can  be  converted  to  a  distance  by  simply  multiplying  by  c.  The  distance  calculations  are 
biased  by  the  receiver  and  satellite  clock  errors,  and  are  therefore  referred  to  as  pseudo¬ 
ranges.  Because  of  cost  and  other  constraints,  the  receiver  clock  is  in  general  much  less 
accurate  than  the  satellite  clocks.  If  the  location  of  four  in-view  satellites  is  known,  and 
their  ranges  to  the  receiver  are  measured,  the  location  of  the  receiver  and  its  clock  bias 
can  be  computed. 

The  objective  of  this  report  is  to  present  the  mathematics  used  to  convert  the  satellite-to- 
receiver  pseudoranges  to  receiver  position  estimates.  The  report  discusses  a  method  that 
is  used  to  determine  instantaneous  estimates  of  receiver  position,  i.e.,  estimates  based  on 
pseudo-ranges  at  one  time  instant,  and  then  goes  on  to  develop  three  Kalman  filter  based 
estimators.  Typically  the  instantaneous  estimates  are  used  to  initialize  a  Kalman  filter,  as 
Kalman  filters  require  an  initial  estimate  to  start  their  recursions.  The  three  Kalman  filter 
estimators  that  are  presented  will  be  referred  to  as  stationary  receiver,  low  dynamics,  and 
high  dynamics  filters.  As  is  implied  by  their  names,  the  three  types  of  filters  are  optimized 
for  situations  where  the  receiver  is  stationary,  is  subjected  to  small  accelerations,  and  to 
large  accelerations,  respectively.  This  approach  is  consistent  with  what  can  be  found  in 
many  actual  GPS  receivers,  which  allow  the  user  to  specify  the  dynamic  level  for  the  given 
application. 

While  the  standard  form  of  the  Kalman  filter,  of  which  the  three  filters  just  mentioned 
are  examples,  is  theoretically  correct,  it  can  be  susceptible  to  numerical  round-off  errors. 
The  effects  of  these  errors  can  be  degraded  filtering  or,  in  some  instances,  Kalman  filter 
instability,  leading  to  quite  unpredictable  behaviour.  This  issue,  and  its  solution,  is  in¬ 
vestigated,  and  another  version  of  the  high  dynamics  filter  is  presented.  Matlab  code  was 
developed  to  test  the  performance  of  each  of  the  filters  and  simulations  performed.  The 
results  of  the  simulations  are  also  presented. 

In  Section  2,  the  development  of  the  three  types  of  Kalman  filter,  as  well  as  the  instanta¬ 
neous  estimator  is  presented.  Section  3  then  presents  the  results  of  testing  by  simulation. 
It  is  noted  that  there  are  some  indications  of  adverse  effects  due  to  numerical  round-off  in 
the  case  of  the  high  dynamics  Kalman  filter.  To  investigate  this  issue  further,  an  alternate 
form  of  the  high  dynamics  filter  is  developed  in  Section  4.  The  filter  was  implemented  in 
Matlab  and  tested  by  simulation;  the  results  of  the  simulations  are  also  in  Section  4.  At 
the  end  of  the  paper,  concluding  remarks  are  presented  in  Section  5. 


UNCLASSIFIED 


l 


DST-Group-TR-3260 


UNCLASSIFIED 


2  Kalman  Filter  Development  for  the  Processing 

of  GPS  measurements 

2.1  Initial  Single  Point  GPS  solution 


The  GPS  positioning  problem  has  four  unknowns  that  can  be  solved  using  the  following 
equations  which  use  measurements  from  four  satellites  [1,  p.  145]. 


(1) 


where 


Xi  =  ctSVi  +  ctai  +  ei  +  rrn  +  ry,  i  =  1,  ..,4 


and  pi,i  =  1, 4  are  the  measured  pseudoranges  from  satellite  i  to  the  receiver  an¬ 
tenna,  Xj  .  Yj,  Zi  are  the  earth-centred-earth- fixed  (ECEF)  position  coordinates  of  satellite 
i,  x ,  y,  z  are  the  ECEF  position  coordinates  of  the  receiver  antenna,  tr  is  the  receiver  clock 
bias,  tSVi  is  the  clock  bias  of  satellite  i,  tai  is  the  atmospheric  delay,  e*  represents  the  error 
in  the  broadcast  ephemeris  data,  rnt  represents  the  multipath  error,  ry  represents  receiver 
tracking  error  noise,  and  c  is  the  speed  of  light. 

In  equations  1,  the  pseudorange  measurements  are  dependent  on  the  receiver  coordinates 
in  a  nonlinear  manner.  While  closed  form  solutions  are  available,  typically  the  solution  is 
found  by  first  linearizing  the  measurement  equations,  which  can  then  be  solved  iteratively. 
The  method  described  below  relies  on  Newton’s  method. 

Let  us  assume  that  Xi  =  0,  i  =  1,  ..,4,  then  the  relationships  between  the  pseudoranges 
and  the  receiver  position  are 


Note  that  in  the  above  equation  it  is  effectively  assumed  that  the  only  source  of  range 
bias  is  the  receiver  clock  bias,  which  can  be  calculated  and  accounted  for  by  solving  four 
simultaneous  equations,  instead  of  the  minimum  of  three  that  would  be  required  if  there 
was  no  range  bias  at  all. 
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Defining  the  vector  x  =  (x,y,  z,  ctr)  and  linearizing  Equations  2  results  in 


’  pi  (x)  ’ 

’  Pi  (xo)  " 

(X  -  T0) 

P2  (x) 

P2  (x0) 

+  J 

(y  -  yo) 

P3  (x) 

P3  (X0) 

(z  -  z0) 

.  PA  (x)  _ 

.  Pa  (xo)  . 

(ctr  (ctr) q) 

where  xo  =  (.to,  yo,  zo,  (ctr) 0)  is  the  point  of  linearization,  hot's  represents  the  higher  order 
terms  in  the  expansion  of  Equations  2,  and 


J  = 


dpi  djn  djn  dpi 

dx  dy  dz  d[ct.r) 

dp2  dp2  dp2  dp2 

dx  dy  dz  d(ct.r) 

dps  dps  dps  ops 

dx  dy  dz  d(c.tr ) 

dpi  dpi  dpi  dpi 

dx  dy  dz  d(ct.r ) 


(ZO, 2/0,20, (cir-)o) 


(4) 


The  partial  derivatives  in  Equation  4  can  easily  be  derived  as 


dpi 

-  (Xi  -  x) 

dx 

(Xi  -  xf  +  ( Yi  -  yf  +  (Zi  -  zf 

1/2 

dpi 

-  {Yi  -  y) 

dy 

(Xt  -  x)2  +  (Yi  -  yf  +  (Zi  -  zf 

1/2 

dpi 

-  (Zi  -  z) 

dz 

(Xi  -  xf  +  (Yi  -  yf  +  (Zi  -  zf 

1/2 

dpi 

d  (ctr) 


Now,  if  we  assume  that  hot’s  =  0  in  Equations  3,  and  Xi  =  0,  i  =  1,  ..,4  in  Equations  1, 
we  can  then  form 


’  Pi  (x)  ' 

’  Pi  ’ 

’  Pi  (x0)  ' 

’  Pi  ’ 

P2  (x) 
P3  (x) 

- 

P2 
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= 
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- 

P2 

~P3 
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.  h  . 

.  Pa  (x0)  _ 

.  h  . 

(x  -  x0) 
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(z  ~  zQ) 


O 

0 

0 

1 

o 

1 _ 

Let 


then  we  have 


’  PI  (x)  ' 

’  Pi  " 

P2  (x) 

P2 

P3  (x) 

P3 

.  PA  (x)  . 

.  P4  _ 
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q(x)  =  e(*o)  +  J 


Rearranging  the  above  equation  we  have 

Q  (x0)  +  J 

Multiplying  by  J~1  gives 

7-1 


0  -  X0) 

( y  -  yo) 

(z  -  Zo) 

( ctr  (ctr)  o) 
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and  rearranging  again  gives 


X 

Xo 
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"cT 

C4- 

-i 

O 

1 _ 

-  J  e(x o) 


which  is  now  in  a  suitable  form  for  applying  Newton’s  method  (also  known  as  the  Newton- 
Raphson  method)  by  just  replacing  x  =  (x,  y,  z,  ctr )  with  Xj+i  =  (^£j+i .  Zj+i,  ( ctr)j+1 

and  x0  =  (x0,  yo,  z0,  ( ctr)0 )  with  xj  =  (xj,yj,  Zj ,  {ctr)^j  ,  j  =  0, 1, N  resulting  in 


Xj+i  =xj  -  J  g{xj) 


(6) 


We  simply  start  with  an  initial  guess  for  xo  =  (xo,  yo,  zo,  ( ctr)0 )  and  iterate  till  convergence 
is  reached.  A  simple  test  for  convergence  is  ||xj+i  —  xyj|  <  e,  where  e  is  set  to  a  small 
positive  value. 

If  measurements  from  more  than  four  satellites  are  available,  then  J-1  in  Equation  6  can 
be  replaced  with  (JTJ)  JT  to  give  the  least  squares  solution,  resulting  in 

xj+i  =  Xj  -  (JT jy1  JT Q  (Xj)  (7) 


Equation  7  is  referred  to  as  the  Gauss-Newton  method.  Note  that  Equation  7  assumes 
that  the  pseudorange  measurement  errors  have  identical  variances. 
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2.2  Receiver  Clock  Bias  Dynamic  Model 

One  of  the  components  of  the  Kalman  filter  models  that  are  developed  in  this  report  is 
the  receiver  clock  bias  model.  The  state  space  model  used  for  the  receiver  clock  bias  is 
that  described  on  page  152  of  [1].  The  discrete  time  state  transition  equation  is 


tr(k  +  1) 

'IT' 

tr(k) 

+ 

Ud</>(k) 

tr(k  +  1) 

0  1 

tr(k) 

_  Udf{k) 

where  T  is  the  sampling  period,  and  k  is  the  time  index. 


The  covariance  matrix  associated  with  the  disctrete-time  process  noise  vector  [  ujd<f,(k) 
is 


Qdt  (k) 


S6T+^Sf 


T2  c 

2  A 


SfT 


Udf{k) 


T 


where  and  Sf  are  the  power  spectral  densities  of  and  ojf  (the  continuous-time  process 
noises)  respectively.  An  example  value  of  the  discrete  time  process  noise  covariance  matrix, 
scaled  to  metres,  is  shown  on  page  153  of  [1].  It  is 


Qd  (k)  =  c2Qdt  (k) 


0.0114  0.0019 
0.0019  0.0039 


(8) 


2.3  Plant  and  Measurement  Equations  for  a  Stationary  Re¬ 
ceiver 

Before  going  further,  a  comment  on  notation  is  required.  Many  of  the  equations  de¬ 
scribed  in  this  report  contain  matrices  and  vectors  whose  elements  are  functions  of  time 
(represented  by  the  time  index  k).  To  shorten  the  equations  somewhat,  a  shorthand 
notation  is  used  where  appropriate;  viz.,  consider  an  m  x  n  matrix  A,  with  elements 
ciij  (k) ,  i  =  1, ..,  m.  j  =  1, ..,  n,  then 


an  ...  a\n 

an  (k)  .  . 

■  ain(k) 

Q"ml  •  •  •  & mn 

k 

a-mi  (^) 

O'mn  {k) 

As  can  be  found  on  page  105  of  [1]  (although  full  details  are  not  given  there),  the  discrete 
time  state  transition  equation  that  is  used  for  the  stationary  receiver  case  is 

x  (A;  +  1)  =  F  (k)  x  ( k )  +  v  ( k ) 

where 

x  (k)  =  [x,y,  z,  rtr ,  ftr\k ,  nr  (k)  =  ctr  (k)  (9) 
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F  {k)  is  the  state  transition  matrix,  v  (k),  k  =  0,1, is  a  sequence  of  five  dimensional 
zero  mean  white  Gaussian  process  noise,  and 


F(k) 


1  0  0  0  0 

0  10  0  0 

0  0  10  0 
0  0  0  1  T 

0  0  0  0  1 


(10) 


The  associated  process  noise  covariance  matrix  (with  the  clock  bias  scaled  to  metres)  that 
is  shown  on  page  105  of  [1]  as  an  example  is 


Qv  (k) 


0  0  0  0  0 

0  0  0  0  0 

0  0  0  0  0 

0  0  0  0.07  0.04 

0  0  0  0.04  0.08 


Note  that  the  first  three  elements  in  the  leading  diagonal  of  the  above  matrix  are  zero; 
this  is  because  the  model  assumes  that  the  receiver  is  stationary. 


The  above  covariance  matrix  is  somewhat  different  to  what  would  result  from  Equation 
8,  i.e.: 


Qv  ( k ) 


0  0  0  0  0 

0  0  0  0  0 

0  0  0  0  0 

0  0  0  0.0114  0.0019 

0  0  0  0.0019  0.0039 


(11) 


This  latter  covariance  matrix  is  used  in  the  simulations.  The  most  appropriate  values  for 
the  four  bottom  right  hand  elements  of  the  matrix,  of  course,  depend  on  the  details  of  the 
particular  receiver  that  is  being  modelled. 


It  is  convenient  to  write  the  measurement  equation  in  component  form.  In  this  form  it  is 


Pi  ( k  +  1) 


for  i 


(Aj  {k  +  1)  —  x  (k  +  1))  +  (Yi  {k  +  1)  —  y  (k  +  1))  +  (k  +  1)  —  z  (k  +  1)) 
+rtr  (k  +  1) 

1  ,-,Ns 


Note  that  the  measurement  vector  at  time  k  is 


P  (k)  =  [  Pi  (k)  P2  (k) 


PNS  (k) 


(12) 


For  small  increments  in  Ax  we  can  linearize  as  follows.  Let  Ax  (k  +  1)  =  x  (k  +  1)  —  x  (k), 
then  in  component  form  we  can  write 

A  pi  (k  +  1)  =  Hi  (k  +  1)  Ax  (k  +  1)  +  Vi  (. k  +  1) 

or  in  vector  form  it  is 


A p  (. k  +  1)  =  H  (k  +  1)  Ax  (k  +  1)  +  v  [k  +  1) 
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where 

H  (k  +  1)  = 


dpi 

dpi 

dpi 

dpi 

0  ' 

dx 

dy 

dz 

drtr 

dp2 

dpi 

dp2 

dp2 

0 

dx 

dy 

dz 

drtr 

dpN, 

dpNs 

dpNs 

dpNa 

0  _ 

dx 

dy 

dz 

drtr 

fc+1 

(13) 


is  the  5  xJVs  dimensional  measurement  matrix,  and  Ns  is  the  number  of  satellite-to-receiver 
pseudorange  measurements  at  time  k. 

The  partial  derivatives  in  the  above  matrix  are 


dpi 

dx 

dpi 

d  y 

dpi 

dz 

dpi 

dr* 


-  (Xi  -  x) 


1 1/2 


1 1/2 


=  1 


(Xi  -  xf  +  (Yi  -  yf  +  ( Zi  -  zf 
_ _ -(Yj-y) 

(Xi  -  xf  +  (Yi  -  yf  +  (Zi  -  zf 
-  (Zi  -  z) 

9  9  9l  V2 

(Xi  -  xf  +  (Yi  -  yf  +  (Zi  -  zf 

where  i  =  1, Ns 


The  corresponding  measurement  noise  covariance  matrix  is 


R  (k) 


"  of, 

0 

0 

0 

ar2 

.  .  0 

0 

0 

•  •  arN 

r  Ns  -1 

(14) 


where  cf..  ( k ) ,  i  =  1, Ns  is  measurement  noise  variance  for  the  measurement  from  satel¬ 
lite  i  at  time  k. 


2.4  Plant  and  Measurement  Equations  for  a  Low  Dynamics 
Receiver 

A  short  description  of  the  continuous  time  low  dynamics  receiver  model  can  be  found 
on  page  243  of  [1]  (although  full  details  are  not  given  there).  We  need  to  use  a  discrete 
time  model;  a  good  approximation  to  the  corresponding  discrete  time  model  is  as  follows 
[2,  Section  6.3.2],  In  this  model  the  receiver’s  acceleration  is  modelled  using  piecewise 
constant  white  acceleration  noise.  The  discrete-time  state  transition  equation  is 

x  (k  +  1)  =  F  (k)  x  (. k )  +  T  (. k )  v  (. k ) 
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,\  rjp  ^ 

where  T  ( k )  is  noise  gain  at  time  k,  x  ( k )  =  [x,  i,  y,  y,  z,  z.  rtr,ftr]k  ,  rtr  (k)  —  ctr  (k),  and 


F(k)  = 


i  r  o  o  o  o  o  o 
oioooooo 

0  0  1  T  0  0  0  0 
0  0  0  1  0  0  0  0 
0  0  0  0  1  T  0  0 
0  0  0  0  0  1  0  0 
0  0  0  0  0  0  1  T 
0  0  0  0  0  0  0  1 


(15) 


r  (k)  = 


0.5  T2 
T 
0 
0 
0 
0 
0 
0 


0 

0 

0.5T2 

T 

0 

0 

0 

0 


0  0 
0  0 
0  0 
0  0 


0.5T2  0  0 
T  0  0 
0 


0 


1  0 
0  1 


(16) 


(k)  =  [  x  y  z  cujdrt>  cwdf  ], 


(17) 


Let  us  now  determine  the  process  noise  covariance  matrix  associated  with  this  model.  First 
let  us  consider  the  covariance  matrix  associated  with  v  (k),  i.e. ,  Qv  ( k )  =  E  jv  (k)  v  (k)T  j. 
We  have 


Qv  (k) 


cjI  0  0  0  0 

0  cr|  0  0  0 

0  0  erf  0  0 

0  0  0  <7^  ar^arf 

0  0  0  (TrfPrt  &rf 


(18) 


where  Gy  and  o^  are  the  standard  deviations  of  the  x,  y,  and  z  components  of  the 
acceleration  noise,  respectively,  ar<.  and  aTf  are  the  standard  deviation  of  the  clock  bias 
process  noise  due  to  the  phase  error  (scaled  to  metres),  and  that  due  to  frequency  error, 
respectively,  and  arcj>arf  =  gT} a Trb  are  their  covariances.  Note  that  the  components  of  the 
acceleration  noise  are  assumed  to  be  independent  of  each  other  and  the  clock  bias  noises. 


Now  consider  the  process  noise  when  multiplied  by  the  gain  matrix  T  (fc),  i.e.,  Qrv  (k)  = 
E  |  [r  (k)  v  (k)]  [r  (k)  v  (k)]T  |.  The  resulting  process  noise  covariance  matrix  can  be  shown 
to  be 

Qrv{k)  =  T(k)Qv(k)F(k)T  (19) 


The  measurement  equation  is  the  same  as  in  Section  2.3,  but  with  the  measurement  matrix 
now  being 
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dpi 

0 

dpi 

0 

dpi 

0 

dpi 

dx 

dy 

dz 

drtr 

dp2 

0 

dp2 

0 

dp2 

0 

dp2 

dx 

dy 

dz 

drtr 

H(k  +  1)  = 


(20) 


dpNa  n  dpNs 

l  dx  u  dy 


0 


dPNs 

dz 


0 


9pns 

drtr 


0 


k+ 1 


and  the  corresponding  measurement  noise  covariance  matrix  is  as  in  Equation  14. 


2.5  Plant  and  Measurement  Equations  for  a  High  Dynamics 
Receiver 


A  short  description  of  the  continuous  time  high  dynamics  receiver  model  can  be  found  on 
page  244  of  [1]  (although  full  details  are  not  given  there).  We  need  to  use  a  discrete  time 
model;  for  this  we  will  use  the  Wiener  process  acceleration  model  described  in  Section 
6.2.3  of  [2];  this  model  is  also  sometimes  called  the  white  noise  jerk  model.  Note  that  this 
is  a  discretized  continuous  time  model,  as  opposed  to  the  direct  discrete  time  model  of 
Section  6.3.3  of  [2],  Which  of  these  two  models  to  use  is  a  matter  of  choice;  both  are  an 
approximation  to  the  actual  continuous  time  dynamics  of  the  receiver. 

The  discrete-time  state  transition  equation  is 


where 


and 


x  {k  +  1)  =  F  (k)  x  (k)  +  v  (k) 


rri 

x  (k)  =  [x,  x,  x,  y,  y,  y,  z,  z,  z,  rtr,ftr]k  ,  nr  (k)  =  ctr  {k) 


F  (k) 


1  T  \T2  0  0  0  00  0  00 

01  ‘TOO  0  00  0  00 

00  100  000  000 

00  0  1  T  ±T2  0  0  0  00 

00  0  01  TOO  0  00 

00  000  100  000 

00  0  00  0  IT  It2  00 

00  0  00  0  01  ‘too 

00  000  000  1  00 

00  000  000  0  IT 

00  000  000  001 


(21) 


(22) 


With  regard  to  the  process  noise  model,  let  us  first  consider  the  x  component  for  the 
continuous  time  system.  The  changes  in  acceleration  are  modelled  by  a  continuous  time 
zero-mean  white  noise  as  follows 

x  {t)  =  vx  (t) 


Note  that  the  acceleration  is  a  Wiener  process,  and  its  derivative,  the  jerk,  is  represented 
by  a  white  noise  model.  The  changes  in  acceleration  over  a  sampling  period  T  are  of 
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the  order  of  \J qxT ,  where  qx  is  the  power  spectral  density  of  the  continuous  time  process 
noise  vx  ( t ).  The  same  can  be  done  for  the  y  and  £  components.  Considering  all  three 
components  as  well  as  the  receiver  clock  error  noise  model  of  Section  2.2,  we  have  the 
following  discrete  time  process  noise  covariance  matrix  (i.e. ,  Qv  ( k )  =  E  jv  ( k )  v  (fc)r|) 


Qv  (k)  = 


where 


Qx 

o 

0 

0 

0 

0 

0 

0 

0 


0 

0 

0 

0 

0 

0 

0  0 
0  0 
0  0 
0  0 
0  0 


Qx  — 


Qy  — 


Qz  = 


0 

0 

0 

Qy 


0  0 
0  0 
0  0 
0 
0 
0 


0 

0 

0 

0 

0 

0 


0 

0 

0 

0 

Qz 

0 

0 

0 

0  0 

0  0 

0 

0  0 

0  0 

1  t^5 
20  ± 

1  ’-pA 

rTs 

6 1 

rT2 

2 1 

1  rp  3 

T 

1  7^5 
20 

1  rpA 

rT3 

1 0^4 

rTs 

VT2 

2  ± 

1 7^3 

rT2 

2 1 

T 

1  7^5 
20 

1  rpA 

\T3 

1  'T'A 
\T3 

rT2 

2 

1 7^3 

rT2 

2 1 

T 

0 

0 

0 

0 

0 

0 

0 

0 

0 

< 

rr/<Jr-0 


Qx 


Qy 


Qz 


0 

0 

0 

0 

0 

0 

0 

0 

0 

(Tr4>IJrS 


a, 


rf 


(23) 


and  qx,  qy  and  qz  are  the  power  spectral  densities  of  the  x,  y  and  z  components  of  the  con¬ 
tinuous  time  jerk  noise,  i.e.,  vx  (t)  ,vy  (t)  and  vt  (t), respectively.  Note  that  the  components 
of  the  jerk  noise  are  assumed  to  be  independent  of  each  other  and  the  clock  bias  noises. 

The  measurement  equation  is  the  same  as  in  Section  2.3,  but  with  the  measurement  matrix 
now  being 


1 

0 

0 

dpi 

dy 

0 

0 

dpi 

dz 

0 

0 

dpi 

drtr 

0  ' 

dp2 

dx 

0 

0 

dp2 

dy 

0 

0 

dp2 

dz 

0 

0 

dp2 

drtr 

0 

dpNa 

dx 

0 

0 

dPNs 

dy 

0 

0 

dpNs 

dz 

0 

0 

dpNs 

dnr 

0  _ 

fc+i 

and  the  corresponding  measurement  noise  covariance  matrix  is  as  in  Equation  14. 


(24) 


2.6  Extended  Kalman  Filter  Equations  -  General  Case 


An  extended  Kalman  filter  [2,  Section  10.3]  is  used  as  the  estimation  algorithm  in  this 
work.  The  algorithm  is  summarized  in  the  sequel;  before  summarizing  the  algorithm,  some 
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definitions  will  first  be  given,  as  follows.  Let 


where 


x(j|fc)  =  E  x(j)  |Z 


Zk  =  {z  (j),  j<k} 


denotes  the  sequence  of  observations  available  at  time  k,  and 


x(j)  \Zk 


is  the  conditional  expectation  of  x  (j)  at  time  j  given  Zk. 

If  j  =  k ,  then  x  (j\k)  is  the  estimate  of  the  state  (also  called  the  filtered  value);  if  j  =  k  +  1 
then  x  (j\k)  is  the  predicted  value  (one-step)  of  the  state.  The  state  estimation  error  at 
time  k  is  defined  as 

x  (k\k)  =  x  (k)  —  x  (k\k) 

The  state  prediction  error  at  time  k  is  defined  as 

x  (k  +  l|fc)  =  x  (k  +  1)  —  x  [k  +  1|&) 

The  state  estimation  covariance  matrix  (i.e.,  the  covariance  associated  with  the  estimate 
x  (k\k)  )  at  time  k  is 

P  {k\k)  =  e\x  (k\k)  x  ( k\k)T \Zk 

The  state  prediction  covariance  matrix  (i.e.,  the  covariance  associated  with  the  prediction 
x  (k  +  l\k)  )  at  time  k  is 

P  (k  +  l|fc)  =  E  |x  (k  +  l\k)  5t(k  +  1| k)T \Zk 

The  predicted  measurement  (one-step)  is 

z(k  +  l\k)  =  E  z  {k  +  1)  |Z 

The  measurement  prediction  error  (also  called  the  innovation  or  residual)  is  defined  as 
v  (k  +  1)  =  z(k  +  l\k)  =  z  {k  +  1)  —  z  {k  +  l|fc) 

The  measurement  prediction  covariance  matrix  or  innovation  covariance  matrix  is 

S  (k  +  1)  =  E  \z  (k  +  l|fc)  z  (k  +  1| k)T  |Zfcl 
The  Kalman  filter  gain  is 


W  [k  +  1)  =  P  {k  +  l\k)  H  (k  +  1)T  S  (k  +  1) 


-i 


Now  consider  the  nonlinear  system  with  dynamics 

x  (k  +  1)  =  /  [ k ,  x  ( k ) ,  u  ( k)\  +  v  ( k ) 
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where  x  is  the  state  vector,  u  is  a  known  input,  v  is  the  process  noise,  which  is  assumed 
to  be  additive,  zero  mean,  and  white,  and  /  is  a  vector-valued  and  possibly  time  varying 
non-linear  function. 

Let  the  measurement  equation  be 

z  (, k )  =  h  [k,  x  (k)]  +  w  (k) 

where  w  is  the  measurement  noise,  which  is  additive,  zero  mean,  white,  and  uncorrelated 
with  the  process  noise,  and  the  function  h  is  also  vector  valued  and  can  be  time  varying. 

The  Extended  Kalman  filter  is  a  suboptimal  recursive  algorithm  for  the  above  system,  as 
follows.  First,  we  start  with  the  initial  estimate  x(0|0)  of  x  (0)  and  the  associated  initial 
covariance  P  (0|0),  both  assumed  to  be  available.  Then,  for  estimation  of  the  state  of  the 
system,  starting  with  the  state  estimate  x  ( k\k )  at  tk  we  have 

State  Prediction: 
x  ( k  +  l\k)  =  f  [k,  x  ( k\k ) ,  u  (k)] 

Measurement  Prediction: 

z(k  +  l\k)  =  h  [k  +  1,  x  (k  +  l|fc)] 

Measurement  Residual: 

v  {k  +  1)  =  z  {k  +  1)  —  z  {k  +  l|fc) 

Updated  State  Estimate: 

x  [k  +  l\k  +  1)  =  x  (A;  +  l|fc)  +  W  (k  +  1)  v  (k  +  1) 

For  state  covariance  computation,  starting  with  the  state  covariance  P  (k\k)at  tk  we  have 

Evaluation  of  Jacobians: 

 df(k) 


x=x(k-\-l\k) 

State  Prediction  Covariance: 

P(k  +  l\k)  =  F  (k)  P  {k\k)  Ft  (k)  +  Q  (k) 

Residual  Covariance: 

S  {k  +  1)  =  H  (k  +  1)  P  (k  +  1|  k)H{k+  if  +  R(k  +  1) 
Filter  Gain: 

W  (k  +  1)  =  P  [k  +  l|ife)  H  ( k  +  if  S  ( k  +  1  )_1 

Updated  State  Covariance: 

P  (k  +  l\k  +  1)  =  P  (k  +  l|fe)  -W(k  +  l)S(k  +  l)W(k  +  if 
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Oh  (k  +  1) 


dx 


UNCLASSIFIED 


DST-Group-TR-3260 


2.7  Extended  Kalman  Filter  Equations  for  a  Stationary  Re¬ 
ceiver 


The  extended  Kalman  filter  for  the  stationary  receiver  case  described  in  Section  2.3  will 
now  be  described. 


We  start  with  the  initial  estimate  x  (0|0)  of  x  (0)  which  is  determined  using  the  equations 
presented  in  Section  2.1.  Note  that  the  initial  estimate  doesn’t  give  any  information  about 


the  rate  of  change  of  receiver  clock  bias  -  this  needs  to  be  guessed.  Our  initial  guess  is 


that  it’s  zero.  We  also  need  to  calculate  the  covariance,  P(0|0) ,  of  the  initial  estimate 
x  (0|0),  which  can  be  determined  as  follows.  Let  Pa  be  the  covariance  associated  with  the 


estimate  of  ( x,y,z,ctr )  obtained  using  Equation  7.  Referring  to  Equation  4.11  in  section 
4.1.1  of  [1]  we  have 


where 


J(0) 


PA={ 

J(0)T 

p(o)-1 

J(0)) 

r  dpi 

dpi 

dpi 

dpi 

dx 

9y 

dz 

drtr 

dp2 

dp2 

dp2 

dp2 

dx 

dy 

dz 

drtr 

dPNs 

dPNs 

dpNa 

dpNs 

dx 

dy 

dz 

drtr 

-l 


x=x(0|0) 


(25) 


(26) 


Then 


R(  o) 


°n  0 

0  <t2T2 


0 

0 


0 


0 


a;=x(0|0) 


P(0|0) 


PAu 

PAi  2 

PAl3 

PA14 

0 

Pa21 

P A  2  2 

PA23 

pa24 

0 

PA3 1 

PA32 

PA33 

PA3  4 

0 

PA4 1 

PA42 

PA43 

pa44 

0 

0 

0 

0 

0 

2  Qd, 

(27) 


Note  that  in  the  above  matrix  the  covariances  of  the  fifth  column  and  fifth  row  can’t  be 
determined  from  the  measurements  made  on  the  initial  startup,  hence,  as  a  reasonable 
guess,  they  are  all  set  to  zero,  and  the  variance  in  the  bottom  right  hand  corner  equal 
to  2 Qd22i  i-e->  twice  the  variance  of  element  22  of  the  discrete  time  receiver  clock  process 
noise  covariance  matrix,  scaled  to  metres,  in  Equation  8. 


Then,  for  estimation  of  the  state  of  the  system,  starting  with  the  state  estimate  x  ( k\k )  at 
tk  we  have 


State  Prediction: 
x  (k  +  l\k)  =  F  ( k )  x  ( k\k ) 


(28) 
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Measurement  Prediction: 


Pi  ( k  +  l\k) 


(Xi  (k  +  1)  -  x  (. k  +  1| k))2  +  (Yi  ( k  +  1) 


+  ftr  (k  +  1|  k) 
for  i  =  1, Ns 


y  ( k  +  1|  A:))2  +  (Zi  (k  +  1)  -  z  (k  +  1| k))2 

(29) 


Measurement  Residual: 


v  {k  +  1)  =  p{k  +  1)  -  p  ( k  +  1| k) 
Updated  State  Estimate: 

x  [k  +  l\k  +  1)  =  x  (A;  +  l|fc)  +  W  (k  +  1)  v  (k  +  1) 


(30) 

(31) 


For  state  covariance  computation,  starting  with  the  state  covariance  P  (k\k)at  A  we  have 


Evaluation  of  Jacobians: 


r  dpi 

dpi 

dpi 

dpi 

dx 

dy 

dz 

drtr 

dp2 

dp2 

dp2 

dp2 

dx 

dy 

dz 

drtr 

H(k  +  1)  = 

dPNa 

dpNs 

dPNs 

9pN3 

dx 

dy 

dz 

drtr 

x=x(fc+l|/c) 


State  Prediction  Covariance: 

P  (k  +  l|fc)  =  F  (k)  P  (k\k)  Ft  (k)  +  Qrv  (. k ) 


Residual  Covariance: 

S  {k  +  1)  =  H  (k  +  1)  P  (k  +  l\k)  H  {k  +  1)T  +  R  (k  +  1) 


Filter  Gain: 

W  (k  +  1)  =  P  [k  +  l|ife)  H  (k  +  1)T  S  ( k  +  l)"1 


(32) 

(33) 

(34) 

(35) 


Updated  State  Covariance: 

P  {k  +  l\k  +  1)  =  P  (k  +  1| k)  -W(k  +  l)S(k  +  l)W(k  +  1)T  (36) 

where  x(/c),  F  ( k ),  Qrv  ( k ),  p(k)  and  R(k)  are  dehned  in  Equations  9,  10,  11,  12  and  14, 
respectively. 
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2.8  Extended  Kalman  Filter  Equations  for  a  Low  Dynamics 
Receiver 


The  extended  Kalman  filter  for  the  low  dynamics  receiver  case  will  now  be  described. 

Again,  we  start  with  the  initial  estimate  x(0|0)  of  x  (0)  which  is  determined  using  the 
equations  presented  in  Section  2.1.  Since  the  the  initial  estimate  doesn’t  give  any  infor¬ 
mation  about  the  rate  of  change  of  receiver  clock  bias,  this  needs  to  be  guessed.  Our  initial 
guess  is  that  it’s  zero.  We  also  need  to  calculate  the  covariance,  P  (0|0) ,  of  the  initial  es¬ 
timate  x  (0|0),  which  can  be  determined  as  follows.  As  in  Section  2.7,  Equation  25  is  used 
to  determine  Pa,  with  J  (0)  and  R  (0)  defined  as  in  Equations  26  and  27,  respectively. 
Note  that  Pa  is  the  covariance  associated  with  the  estimate  of  (x,  y,  z,  ctr)  obtained  using 
Equation  7. 

Then 


P(0|0) 


’  PAU 

0 

PAV2 

0 

0 

ai  (0) 

0 

0 

PA2i 

0 

pa22 

0 

0 

0 

0 

^(°) 

PA3 1 

0 

PA32 

0 

0 

0 

0 

0 

PAU 

0 

PAi2 

0 

L  0 

0 

0 

0 

PA13 

0 

PA14 

0 

0 

0 

0 

0 

pa23 

0 

PA24 

0 

0 

0 

0 

0 

pa33 

0 

PA3  4 

0 

0 

(0) 

0 

0 

PA.,3 

0 

pAAA 

0 

0  0  0  2 Qd22  \ 


Note  that  in  the  above  matrix  the  covariances  (i.e.,  the  off-diagonal  terms)  of  the  second, 
fourth,  sixth  and  eight  row  as  well  as  the  second,  fourth,  sixth  and  eighth  column  can’t 
be  determined  from  the  measurements  made  on  the  initial  startup,  hence,  as  a  reasonable 
assumption,  they  are  all  set  to  zero.  The  variances  of  the  x,  y  and  z  components  of  the 
initial  velocity  estimate  are  assumed  to  be  <tJ  (0) ,  <r|  (0)  and  erf  (0)  respectively,  and  the 
bottom  right  hand  element  of  P(0|0)  is  set  to  2Qd22,  he.,  twice  the  variance  of  element 
22  of  the  discrete  time  receiver  clock  process  noise  covariance  matrix,  scaled  to  metres,  in 
Equation  8. 

Then  for  estimation  of  the  state  of  the  system,  starting  with  the  state  estimate  x(k\k)  at 
tk  and  for  state  covariance  computation,  starting  with  the  state  covariance  P  (k\k)  at  A 
we  use  Equations  28  to  31  and  33  to  36,  where  x  (k),  F  (k),  T  (k),  Qrv  (k),  p  (k)  and  R  (k) 
are  defined  in  Equations  2.4,  15,  16,  19,  12  and  14,  respectively  and  H  ( k  +  1)  is  given  by 


dpi 

0 

dpi 

0 

dpi 

0 

dpi 

dx 

dy 

dz 

drtr 

dp2 

0 

dp2 

0 

dp2 

0 

dp2 

dx 

dy 

dz 

drtr 

H  (k  +  1)  = 


(37) 


dpNs 

dx 


n  dPNs 
U  dy 


0 


dpNs 

dz 
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dpNs 

drtr 


0 


x=x(/c+l|/c) 
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2.9  Extended  Kalman  Filter  Equations  for  a  High  Dynamics 
Receiver 


The  extended  Kalman  filter  for  the  high  dynamics  receiver  case  will  now  be  described. 
Again,  we  start  with  the  initial  estimate  x(0|0)  of  x  (0)  which  is  determined  using  the 
equations  presented  in  Section  2.1.  Since  the  the  initial  estimate  doesn’t  give  any  infor¬ 
mation  about  the  rate  of  change  of  receiver  clock  bias,  this  needs  to  be  guessed.  Our  initial 
guess  is  that  it’s  zero.  We  also  need  to  calculate  the  covariance,  P  (0|0) ,  of  the  initial  es¬ 
timate  x  (0|0),  which  can  be  determined  as  follows.  As  in  Section  2.7,  Equation  25  is  used 
to  determine  Pa,  with  J  (0)  and  R( 0)  defined  as  in  Equations  26  and  27,  respectively. 
Note  that  Pa  is  the  covariance  associated  with  the  estimate  of  (x,  y,  z,  ctr )  obtained  using 
Equation  7. 


P(0|0)  = 


Then 
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0 
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0 

0 
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0 

0 

0 

0 

0 

0 

2Qd22 

In  the  above  matrix,  the  covariances  (i.e. ,  the  off-diagonal  terms)  of  the  second,  third, 
fifth,  sixth,  eight,  ninth  and  eleventh  row  as  well  as  the  second,  third,  fifth,  sixth,  eight, 
ninth  and  eleventh  column  can’t  be  determined  from  the  measurements  made  on  the  initial 
startup,  hence,  as  a  reasonable  assumption,  they  are  all  set  to  zero.  The  variances  of  the 
x ,  y  and  z  components  of  the  initial  velocity  estimate  are  assumed  to  be  of  (0) ,  of  (0)  and 
of  (0)  respectively,  the  variances  of  the  x,  y  and  z  components  of  the  initial  acceleration 
estimate  are  assumed  to  be  of-  (0) ,  of  (0)  and  erf  (0)  respectively,  and  the  bottom  right 
hand  element  of  P  (0|0)  is  set  to  2 Qd22,  be.,  twice  the  variance  of  element  22  of  the  discrete 
time  receiver  clock  process  noise  covariance  matrix,  scaled  to  metres,  in  Equation  8. 

Then  for  estimation  of  the  state  of  the  system,  starting  with  the  state  estimate  x(k\k)  at 
tk  and  for  state  covariance  computation,  starting  with  the  state  covariance  P  (k\k)  at 
we  use  Equations  28  to  31  and  33  to  36,  where  x(fc),  F  (k),  Qv  ( k ),  p(k)  and  R(k)  are 
defined  in  Equations  21,  22,  23,  12  and  14,  respectively  and  H  (k  +  1)  is  given  by 


H(k  +  1)  = 
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x=x(/c+l|/c) 
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3  Testing  of  Kalman  Filter  Algorithms 

In  order  to  test  the  algorithms  developed  in  the  previous  sections,  simulations  were  written 
using  the  Matlab  programming  language.  The  aim  of  the  simulations  was  to  test  the 
instantaneous  and  Kalman  filter  estimators  for  stability  and  in  general  determine  if  they 
are  performing  as  expected.  Simpifications  were  made  in  the  scenarios  considered,  to  the 
extent  that  was  possible,  while  still  achieving  the  aims  of  the  testing.  Modelling  of  the 
rotation  of  the  earth,  and  movement  of  the  satellites  was  not  required  for  this  stage  of  the 
testing  and  hence  was  not  implemented  in  the  simulations,  i.e.,  it  was  assumed  that  the 
receiver  was  in  an  arbitrary  inertial  reference  frame  and  the  satellites  were  stationary  in 
this  frame,  with  a  predefined  geometric  configuration  relative  to  initial  receiver  position. 
The  receiver-satellite  geometry  was  made  consistent  with  what  could  be  expected  for  an 
actual  situation.  In  the  testing  of  the  Kalman  filter  using  a  stationary  receiver  model, 
the  receiver  was  kept  stationary,  whereas,  for  the  other  Kalman  filters,  the  receiver  was 
in  motion.  Note  that  further  testing  with  more  sophisticated  scenarios,  utilizing  actual 
satellite  trajectories  would  be  desirable  to  fully  test  the  filters  which  were  developed. 


3.1  Kalman  Filter  with  Stationary  Receiver  Model 

The  Kalman  filter  using  the  stationary  receiver  model,  which  was  described  in  Section 
2.7,  was  coded  in  Matlab  and  tested  by  simulation.  The  details  of  the  simulations  are  as 
follows. 

The  Kalman  filter  update  rate  was  set  to  T  =  1  sec.  The  number  of  updates  that  the 
Kalman  filter  was  run  for  was  300  to  determine  short  term  performance,  and  then  3600 
to  determine  performance  over  a  longer  period  of  time  (1  hour).  The  latter  served  as  a 
more  extended  test  to  determine  if  there  are  any  issues  associated  with  filter  divergence 
due  to  numerical  round-off  errors,  which  is  a  common  problem  in  Kalman  filter  imple¬ 
mentations.  The  “actual”  receiver  range  measurement  error  standard  deviation  was  set  to 
aVa  =  5m.  The  receiver  range  measurement  error  standard  deviation  as  modelled  by  the 
Kalman  filter  was  set  to  oym  =  5m.  Six  satellites  were  modelled  in  the  simulations;  their 
positions  were  xS|  =  (0.9390,-1.6265,1.8781)  X  107m,  xS2  =  (1.7648,-0.6423,1.8781)  x 
107m,  xS3  =  (1.7648,0.6423, 1.8781)  x  107m,  xS4  =  (0.9390, 1.6265, 1.8781)  x  107m,  xS5  = 
(0.9390,-1.6265,-1.8781)  x  107m,  xS6  =  (0.9390,1.6265,-1.8781)  x  107m.  The  receiver 
position  was  xr  =  (6.371  X  106, 100, 150)m. 

Figures  1  and  2  show  the  results  for  the  case  of  300  updates.  Figure  1  shows  the  errors 
in  the  instantaneous  position  estimates;  the  instantaneous  estimates  were  calculated  using 
the  Gauss-Newton  method  as  described  in  Section  2.1.  Figure  2  shows  the  errors  in 
the  Kalman  filter  estimates.  Note  that  the  error  is  defined  to  be  the  distance  between 
the  estimated  position  and  the  actual  position.  Looking  at  Figure  2,  we  see  that  the 
Kalman  filter  is  very  quickly  reducing  the  position  estimate  errors  to  well  below  that  of 
the  instantaneous  estimates. 

Figures  3  and  4  show  the  results  for  the  case  of  3600  updates;  The  primary  reason  for 
doing  the  simulation  that  resulted  in  these  figures  was  to  test  for  divergence  that  may 
result  from  numerical  round-off  errors.  As  can  be  seen  from  the  two  figures,  convergence 
continued  for  the  duration  of  the  simulation. 
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Figure  1:  Position  estimation  error  of  instantaneous  estimates  (stationary  receiver  model, 
300  updates). 


Figure  2:  Kalman  filter  position  estimation  error  (300  updates),  using  stationary  receiver 
model. 
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Figure  3:  Position  estimation  error  of  instantaneous  estimates  ( stationary  receiver  model, 
3600  updates). 


Figure  f:  Kalman  filter  position  estimation  error  (3600  updates),  using  stationary  receiver 
model. 
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3.2  Kalman  Filter  with  Low  Dynamics  Receiver  Model 

The  Kalman  filter  using  the  low  dynamics  receiver  model,  which  was  described  in  Section 
2.8,  was  coded  in  Matlab  and  tested  by  simulation.  The  details  of  the  simulations  are  as 
follows. 

The  Kalman  filter  update  rate  was  set  to  T  =  1  sec.  The  number  of  updates  that  the 
Kalman  filter  was  run  for  was  300  to  determine  short  term  performance,  and  then  3600 
to  determine  performance  over  a  longer  period  of  time  (1  hour).  The  ’’actual”  receiver 
range  measurement  error  standard  deviation  was  set  to  oTa  =  5m.  The  receiver  range 
measurement  error  standard  deviation  as  modelled  by  the  Kalman  filter  was  set  to  oym  = 
5m.  The  (acceleration)  process  noise  standard  deviation  in  the  Kalman  filter  was  set 
to  (Jx  =  (Ty  =  (T'z  =  0.2  m/s2.  Six  satellites  were  modelled  in  the  simulation;  their 
positions  were  x,S]  =  (0.9390,-1.6265,1.8781)  x  107m,  xS2  =  (1.7648,-0.6423,1.8781)  x 
107m,  xS3  =  (1.7648,0.6423, 1.8781)  x  107m,  xS4  =  (0.9390, 1.6265, 1.8781)  x  107m,  xS5  = 
(0.9390,-1.6265,-1.8781)  X  107m,  xS6  =  (0.9390,1.6265,-1.8781)  x  107m.  The  initial 
receiver  position  was  xr  =  (6.371  x  106, 100, 150)m;  however,  in  these  simulations  the 
receiver  was  not  stationary,  but  instead  had  a  velocity  of  vr  =  (0,30,40)  m/s  for  the 
duration  of  the  simulations. 

Figures  5  and  6  show  the  results  for  the  case  of  300  updates.  Figure  5  shows  the  errors 
in  the  instantaneous  position  estimates.  Figure  6  shows  the  errors  in  the  Kalman  filter 
estimates.  Looking  at  Figure  6,  we  see  that  the  Kalman  filter  is  quickly  reducing  the 
position  estimate  errors  to  below  that  of  the  instantaneous  estimates.  Note,  however,  that 
the  errors  in  the  position  estimates  are  higher  than  was  the  case  for  the  filter  using  the 
stationary  receiver  model.  This  is  to  be  expected  as  this  filter  allows  for  receiver  motion, 
and  hence  does  not  filter  the  position  estimates  as  heavily.  Of  course,  this  filter  has  the 
advantage  that  it  can  track  the  position  and  velocity  of  a  moving  receiver,  whereas  the 
filter  with  the  stationary  receiver  model  is  not  designed  for  a  moving  receiver,  and  hence 
would  not  be  expected  to  function  well  for  that  case. 

Figures  7  and  8  show  the  results  for  the  case  of  3600  updates.  As  can  be  seen  from  the 
two  figures,  convergence  continued  for  the  duration  of  the  simulation. 


3.3  Kalman  Filter  with  High  Dynamics  Receiver  Model 

The  Kalman  filter  using  the  high  dynamics  receiver  model,  which  was  described  in  Section 
2.9,  was  coded  in  Matlab  and  tested  by  simulation.  The  details  of  the  simulations  are  as 
follows. 

The  Kalman  filter  update  rate  was  set  to  T  =  1  sec.  The  number  of  updates  that 
the  Kalman  filter  was  run  for  was  300  to  determine  short  term  performance,  and  then 
3600  to  determine  performance  over  a  longer  period  of  time  (1  hour).  The  ’’actual”  re¬ 
ceiver  range  measurement  error  standard  deviation  was  set  to  oya  =  5m.  The  receiver 
range  measurement  error  standard  deviation  as  modelled  by  the  Kalman  filter  was  set 
to  oym  =  5m.  The  power  spectral  densities,  qx,%  and  qz,  of  the  x,y  and  z  compo¬ 
nents  of  the  continuous  time  jerk  noise  were  set  to  qx  =  qy  =  qz  =  0.2.  Six  satellites 
were  modelled  in  the  simulation;  their  positions  were  xSl  =  (0.9390,-1.6265,1.8781)  x 
107m,  xS2  =  (1.7648,-0.6423,1.8781)  x  107m,  xS3  =  (1.7648,0.6423,1.8781)  x  107m, 
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Figure  5:  Position  estimation  error  of  instantaneous  estimates  (low  dynamics  receiver 
model,  300  updates). 


Figure  6:  Kalman  filter  position  estimation  error  (300  updates),  using  low  dynamics  re¬ 
ceiver  model. 
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Figure  1:  Position  estimation  error  of  instantaneous  estimates  (low  dynamics  receiver 
model,  3600  updates). 
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Figure  8:  Kalman  filter  position  estimation  error  (3600  updates),  using  low  dynamics 
receiver  model. 


UNCLASSIFIED 

22 


UNCLASSIFIED 


DST-Group-TR-3260 


Update  number 


Figure  9:  Position  estimation  error  of  instantaneous  estimates  (high  dynamics  Kalman 
filter,  300  updates). 

xS4  =  (0.9390, 1.6265, 1.8781)  x  107m,  xS5  =  (0.9390,-1.6265,-1.8781)  x  107m,  xS6  = 
(0.9390, 1.6265, —1.8781)  x  107m.  The  initial  receiver  position  was  xr  =  (6.371  x  106, 100, 150)m. 
The  receiver  was  initially  stationary,  then,  from  t  =  101  s  to  t  =  200  s,  it  experienced  an 
acceleration  of  ar  =  (0,3,4)  m/s2. 

Figures  9  and  10  show  the  results  for  the  case  of  300  updates.  Figure  9  shows  the  errors 
in  the  instantaneous  position  estimates.  Figure  10  shows  the  errors  in  the  Kalman  filter 
estimates.  Looking  at  Figure  10,  we  see  that  the  Kalman  filter  is  quickly  reducing  the 
position  estimate  errors  to  below  that  of  the  instantaneous  estimates;  however,  the  errors 
in  the  position  estimates  are  higher  than  was  the  case  for  the  filters  using  the  stationary 
receiver  and  low  dynamics  models.  Also,  note  that  this  filter  has  the  advantage  that  it 
can  track  the  position,  velocity  and  acceleration  of  the  receiver.  Figures  11  and  12  show 
the  velocity  and  acceleration  estimates,  respectively,  for  the  case  of  300  updates  (same 
simulation  as  that  which  produced  Figures  9  and  10). 

Figures  13  and  14  show  the  results  for  the  case  of  3600  updates.  Figure  13  shows  the 
errors  in  the  instantaneous  position  estimates,  and  Figure  14  shows  the  errors  in  the 
Kalman  filter  estimates.  Figures  15  and  16  show  the  velocity  and  acceleration  estimates, 
respectively,  for  the  case  of  3600  updates  (same  simulation  as  that  which  produced  Figures 
13  and  14).  As  can  be  seen  from  the  two  figures,  convergence  continued  for  the  duration 
of  the  simulation.  While,  superficially,  the  performance  of  the  high  dynamics  Kalman 
filter  appears  correct,  a  closer  look  indicates  an  anomoly.  Looking  at  Figures  12  and  16, 
we  note  that  the  acceleration  estimates  are  very  heavily  filtered  subsequent  to  about  150 
updates.  Noting  the  power  spectral  densities  used  in  the  Kalman  filter  model  for  the 
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Figure  10:  Kalman  filter  position  estimation  error  (300  updates),  using  high  dynamics 
receiver  model. 
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Figure  11:  Velocity  estimates  of  high  dynamics  Kalman  filter  (300  updates).  The  red, 
green  and  blue  plots  are  the  x,  y  and  z  components  of  the  velocity,  respectively. 


24 


UNCLASSIFIED 


UNCLASSIFIED 


DST-Group-TR-3260 


10  r 


8  H 


6  - 


2  1i 


•  V ' 'A" 


-2  - 

-4  - 

-6  - 

-8  - 

-10  - 

-12  — 
0 


/'/ . 

v- 

n 


f 


.  \ 

\  \ 

\  \ 

\  \ 


50 


100 


150  200 

Update  number 


250 


300 


350 


Figure  12:  Acceleration  estimates  of  high  dynamics  Kalman  filter  (300  updates).  The 
red,  green  and  blue  plots  are  the  x,  y  and  z  components  of  the  acceleration, 
respectively. 


continuous-time  jerk  noise,  i.e.,  qx  =  qy  =  qz  =  0.2,  and  referring  to  Equation  6. 2. 3-6  in 
[2],  we  would  expect  changes  of  acceleration  during  a  sampling  period  T  to  be  of  the  order 
of  \JqxT ,  \JqyT  and  \JqzT  for  the  x,  y  and  z  components  respectively,  i.e.,  y/(L2  ~  0.45 
m/s2.  Hence,  given  noisy  measurements,  we  would  intuitively  expect  that,  after  a  period 
of  convergence,  the  acceleration  estimates  of  the  filter  would  exhibit  acceleration  noise  of 
this  order.  Looking  at  Figures  12  and  16,  we  see  that  the  acceleration  noise  is  well  below 
this,  indicating  that  the  filter  is  filtering  more  heavily  than  it  is  designed  to  do.  A  possible 
cause  of  this  is  numerical  roundoff  error.  This  will  be  investigated  in  the  following  section. 


4  Minimizing  Round-off  Errors 


The  Kalman  filter  implementations  described  up  to  this  point  will,  from  a  theoretical 
standpoint,  give  correct  results  based  on  the  models  used;  however,  in  practice  they  can 
be  somewhat  sensitive  to  computer  round-off  errors.  Round-off  errors  are  a  side  effect  of 
computer  arithmetic  using  a  fixed  number  of  bits  for  representing  numbers.  In  this  chapter 
we  will  consider  an  alternative  implementation  technique  that  significantly  reduces  the 
effects  of  these  errors. 
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Figure  13:  Position  estimation  error  of  instantaneous  estimates  (high  dynamics  Kalman 
filter,  3600  updates). 
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Figure  If:  Kalman  filter  position  estimation  error  (3600  updates),  using  high  dynamics 
receiver  model. 
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Figure  15:  Velocity  estimates  of  high  dynamics  Kalman  filter  (3600  updates).  The  red, 
green  and  blue  plots  are  the  x,  y  and  z  components  of  the  velocity,  respectively. 
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Figure  16:  Acceleration  estimates  of  high  dynamics  Kalman  filter  (3600  updates).  The 
red,  green  and  blue  plots  are  the  x,  y  and  z  components  of  the  acceleration, 
respectively. 
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4.1  Some  Preliminaries 

There  have  been  various  techniques  developed  as  alternatives  to  the  standard  Kalman 
filter,  with  the  aim  of  reducing  the  effects  of  round-off  errors.  A  description  of  some  of 
the  most  important  techniques  can  be  found  in  Chapter  6  of  [3].  Amongst  these,  the 
most  reliable  and  numerically  stable  implementations  of  the  Kalman  filter  are  collectively 
referred  to  as  square-root  filters  [3,  Section  6.4],  Square-root  filters  use  a  reformulation 
of  the  state  prediction  and  state  estimate  equations  such  that  the  dependent  variable  is 
a  Cholesky  factor,  or  modified  Cholesky  factor  of  the  covariance  matrices  P  ( k  +  1 1 A’)  and 
P  (k  +  l|fc  +  1).  Two  of  the  more  favoured  implementations  of  square-root  filter  are  the 
Carls  on- Schmidt  square-root  filter  and  the  Bierman-  Thornton  UD  filter.  We  will  concen¬ 
trate  on  the  Bierman-Thornton  UD  filter,  as  it,  in  particular,  has  been  used  successfully 
on  problems  with  thousands  of  state  variables  [3,  p.  262]. 

First,  let  us  summarize  what  Cholesky  and  modified  Cholesky  factors  are  [3,  Section  6.4.3], 
The  product  of  a  matrix  C  with  its  own  transpose  in  the  form  CCT  =  M  is  called  the 
symmetric  product  of  C,  and  C  is  called  a  Cholesky  factor  of  M.  Note  that,  strictly 
speaking,  a  Cholesky  factor  is  not  a  matrix  square  root,  although  the  terms  are  often 
used  interchangeably.  All  symmetric  nonnegative  definite  matrices  (of  which  covariance 
matrices  are  an  example)  have  Cholesky  factors.  An  upper  triangular  matrix  U  is  called 
unit  upper  triangular  if  its  diagonal  elements  are  all  1.  Similarly,  a  lower  triangular 
matrix  L  is  called  unit  lower  triangular  if  all  of  its  diagonal  elements  are  1.  The  modified 
Cholesky  decomposition  of  a  symmetric  positive  definite  matrix  M  is  a  decomposition  into 
products  M  =  UDUt  such  that  U  is  unit  upper  triangular  and  D  is  a  diagonal  matrix. 
This  is  also  often  called  UD  decomposition.  The  Bierman-Thornton  UD  filter  relies  on 
UD  decomposition  of  the  covariance  matrices  P  ( k  +  1|A)  and  P  {k  +  l\k  +  1)  to  achieve 
superior  numerical  stability  and  robustness.  The  following  section  describes  this  filter. 


4.2  Bierman-Thornton  UD  Filtering 


For  the  sake  of  compactness,  we  now  introduce  the  following  subscript  notation.  Let  Pm  = 
P(k\k),  Pk+i\k  =  P{k  +  l\k),  and  so  on.  Now,  let  Pk\k  =  Uk\kDk\kUj^k,  and  Pk+1\k  = 
Uk+i\kDk+i\kUk+i\k ■  Consider  one  cycle  of  the  Kalman-filter  covariance  update  now.  The 
state  estimate  error  covariance  matrix  at  time  tk  is  Pk\k  =  Uk\kDk\kUk\k.  Consider  first 
the  temporal  update  of  the  Kalman  filter.  The  state  prediction  covariance  for  cycle  k  +  1 
is 


Pk+l\k  Pk Pk | k Pk  Qk 


Now,  (from  [3,  Section  6. 5. 2. 2]),  let 


A  = 


Vk\k*f  1 

Gk  . 


and 


Dw 


P)k\k  o 

0  L>Qk  . 


Qk  =  GkDQkG 


T 

k 
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where  GkDQkG ^  is  the  modihed  Cholesky  decomposition  of  Qk •  Then 

AT  DWA  =  FkUk^D^U^F^  +  GkDQkGl 

=  FkPk\kFk  +  Qk 

Fk+l\k 

=  Uk+l\kDk+l\kUk+l\k 

Now,  using  the  modified  weighted  Gram-Schmidt  orthogonalization  procedure  ([3,  p.  272]) 
with  respect  to  the  diagonal  weighting  matrix  Dw,  we  produce  a  unit  lower  triangular  nxn 
matrix  L  ,  a  matrix  B ,  and  a  diagonal  matrix  Dp  such  that 


A  =  BL 


and 

BtDwB  =  diag1<i<n  {/3i}  =  Dp 

hence 

AtDwA  =  (BL)T  DWBL 
=  LtBtDwBL 
=  LTDpL 


Consequently,  the  factors 


Uk+l\k  ~  LT 
Dk+l\k  =  Dp 


are  the  solutions  of  the  (Thornton)  temporal  update  problem  for  update  k  of  the  UD 
filter.  Note  that  the  code  that  we  used  for  implementing  this  was  thornton.m  as  supplied 
in  soft-copy  form  with  [3].  It  was  found  in  the  directory  Chapters. 

Now,  let  us  consider  the  measurement  update.  The  updated  state  estimate  covariance  for 
cycle  k  is 

Fk+l\k+l  Fk+l\k  Fk+l\kD k+l^ k+\Dk+lPk+l\k 


where 


<S)c+i  —  Hk+iPk+i\kHk+i  +  Rk+i 


(Equations  2-229  and  2-224  of  [4],  respectively).  Let  us  now  consider  the  case  where  the 
measurement  update  is  a  scalar.  Then  we  have 


Pk+l\k+l  ~  Pk+l\k  ~  -ffc+i|fchfc+1o;fc^1h|1+1Pfc+1|fc 

where  li^+i  is  the  vector  corresponding  to  the  row  of  H^+i  that  applies  to  the  scalar 
measurement  being  considered, 

ak+ 1  =  hfc+iTfc+i|fchfc+i  +  rfc+i 
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and  rk. i-i  is  the  variance  of  the  measurement.  Let  Pk+i\k+i  =  Uk+i\k+iDk-\-i\k+i^k+i\k+\ 
and  Pk+l\k  =  Uk+i\kDk+i\kUl+l^  then  we  have 


T 

Uk+i\k+iDk+i\k+iUk+1\k+i  = 


T 

Uk+i\kDk+1\kUk+1\k 

T  1  T  y 

-  Uk+i\kDk+i\kUk+^khk+iak+1hk+1Uk+i\kDk+i\kUk+1\k 


=  U, 


fc+l|fc 


T  1  jp 

Dk+i\k  ~  Dk+i\kUk+1\khk+1ak+1hk+1Uk+i\kDk+1\k^  Uk+1  |fc 


Let  v  =  Dk+i\kUk+llkhk+i  then 

Uk+i\k+iDk+1\k+1Uk+1\k+1  =  Uk+ i|fc  [Dk+i\k  ~  va^vT]  Uk+1\k 

(note  that  Dk+i |fc+1  =  Dk+1  |fc+1  because  Dk+i |fc+1  is  a  diagonal  matrix). 
Now  perform  UD  decomposition  on  (Dk+1  ik  —  va^v1)  to  get 

Uk+l\k+lDk+l\k+lUk+i\k+l  =  uk+i\k  [UDU1]  Uk+l\k 

=  {Uk+i\kU)D{Uk+1]kU)T 

hence 


E4+I|fe+1  —  Uk+i\kU 
Dk+i\k+i  =  D 

The  algorithim  for  the  UD  decomposition  of  (Dfe+1  \k  —  va^ljV^)  to  produce  UDUT  can 
be  found  on  page  78  of  [5],  and  the  corresponding  Matlab  code  that  was  written  to  imple¬ 
ment  the  algorithm  is  listed  in  Appendix  A. 

Now,  if  the  measurement  is  a  vector,  and  the  measurement  covariance  matrix  is  diagonal, 
then  the  scalar  components  of  the  measurement  can  simply  be  processed  serially  as  scalar 
observations  with  statistically  independent  measurement  errors.  This,  in  fact,  is  the  case 
for  the  measurements  that  we  have.  If  the  measurement  covariance  matrix  is  not  diagonal, 
then  the  components  cannot  be  processed  serially;  however,  the  measurement  vector  can  be 
redefined,  via  a  linear  transformation,  so  that  the  measurement  errors  of  its  components  are 
uncorrelated,  i.e. ,  the  covariance  matrix  of  the  redefined  measurement  vector  is  diagonal. 
A  procedure  for  doing  this  is  described  in  Section  6. 4. 3. 3  of  [3]. 

Now,  to  start  the  Kalman  filter,  we  need  C/o|o  and  D0 10.  To  obtain  these,  we  simply  perform 
UD  decomposition  on  P0|o  as  Per  Section  6. 4. 3. 2  (Table  6.4)  of  [3].  UD  decomposition  is 
also  used  in  one  other  place  in  our  simulation  code,  i.e.,  for  computing  Pa  as  per  Equation 
25.  This  is  done  as  follows;  we  have 

PA=  (jiOfRiOy'JWy1  (39) 

But  R( 0)  =  diag{rj(0)}  is  a  diagonal  matrix,  so  ^(O)”1  =  diag  {1/r*  (0)}  and  hence 
computing  ^  J  (0)T  R  (0)-1  J  (0)^  is  computationally  efficient  and  not  very  sensitive  to 
round-off  errors.  However,  this  is  not  the  case  when  taking  its  inverse.  Circumvention 
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of  this  problem  is  done  as  follows.  Let  Pj  =  ^  J  (0)T  R  (0)_1  J  (0)^  and  now  perform  UD 
decomposition  on  Pj,  resulting  in  Pj  =  UiDjUf ,  hence 

Pa  =  {UjDjUfy1 

=  (ufy1  D-yuy 
=  (uyfpyuy 


Hence,  to  obtain  Pa,  we  now  only  have  to  invert  D,  which  is  a  diagonal  matrix  and  U 
which  is  a  unit  upper  triangular  matrix.  This  turns  out  to  be  less  sensitive  to  numerical 
round-off  errors  than  direct  inversion  of  Pj.  The  reader  is  referred  to  Section  6. 4. 3. 5  and 
Tables  6.4,  6.7  and  6.8  of  [3]  for  a  description  of  the  MATLAB  code  for  doing  this.  Note 
that  the  functions  that  we  used  were  SPDinv.m,  UD -decomp,  m  and  UDinv.m,  as  supplied 
in  soft-copy  form  with  [3].  They  were  found  in  the  directory  TABLE6pt8. 


4.3  Testing  of  Bierman-Thornton  UD  Filter  with  High  Dy¬ 
namics  Receiver  Model 

The  Bierman-Thornton  implementation  of  the  Kalman  filter  with  the  high  dynamics  re¬ 
ceiver  model,  as  described  in  Section  4.2,  was  coded  in  Matlab  and  tested  by  simulation. 
These  simulations  and  tests  were  done  with  the  purpose  of  comparing  the  performance 
with  that  of  the  standard  form  Kalman  Filter  (also  using  the  high  dynamics  receiver 
model).  With  the  exception  of  now  using  the  Bierman-Thornton  implementation,  all 
other  parameters  were  kept  identical  to  those  used  for  the  standard  form  Kalman  Filter 
simulations. 

Figures  17,  18,  19  and  20  show  the  results  for  the  case  of  300  updates.  Figure  17  shows 
the  errors  in  the  instantaneous  position  estimates.  Figure  18  shows  the  errors  in  the 
Bierman-Thornton  Kalman  filter  position  estimates.  Figures  19  and  20  show  the  velocity 
and  acceleration  estimates,  respectively,  for  the  case  of  300  updates  (same  simulation  as 
that  which  produced  Figures  17  and  18). 

Comparing  Figure  18  with  Figure  10,  we  see  that  the  filtered  position  estimate  errors  of  the 
Bierman-Thornton  Filter  appear  to  be  similar  to  that  of  the  standard  form  high  dynamics 
Kalman  filter.  However,  if  we  compare  the  velocity  estimates  in  Figure  19  with  that  of 
Figure  11,  we  note  a  considerable  difference  in  performance.  Comparing  the  acceleration 
estimates  of  Figure  20  with  that  of  12,  we  see  an  even  greater  difference  in  performance. 

Figures  21,  22,  23  and  24  show  the  results  for  the  case  of  3600  updates.  Figure  21  shows 
the  errors  in  the  instantaneous  position  estimates.  Figure  22  shows  the  errors  in  the 
Bierman-Thornton  Kalman  filter  estimates.  Figures  23  and  24  show  the  velocity  and 
acceleration  estimates,  respectively,  for  the  case  of  3600  updates  (same  simulation  as  that 
which  produced  Figures  21  and  22).  Comparing  Figure  22  with  Figure  14,  we  see  that 
the  filtered  position  estimate  errors  of  the  Bierman-Thornton  Filter  appear  to  be  slightly 
smaller  than  that  of  the  standard  form  high  dynamics  Kalman  filter.  As  was  the  case 
for  the  300  update  simulations,  when  we  compare  the  velocity  estimates  in  Figure  23 
with  that  of  Figure  15,  we  note  a  considerable  difference  in  performance.  Comparing  the 
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Figure  1 1:  Position  estimation  error  of  instantaneous  estimates  (high  dynamics  Bierman 
Thornton  Kalman  filter,  300  updates). 


Figure  18:  Bierman- Thornton  Kalman  filter  position  estimation  error  (300  updates),  us 
ing  high  dynamics  receiver  model. 
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Figure  19:  Velocity  estimates  of  high  dynamics  Bierman-  Thornton  Kalman  filter  (300 
updates).  The  red,  green  and  blue  plots  are  the  x,  y  and  z  components  of  the 
velocity,  respectively. 
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Figure  20:  Acceleration  estimates  of  high  dynamics  Bierman- Thornton  Kalman  filter  (300 
updates).  The  red,  green  and  blue  plots  are  the  x,  y  and  z  components  of  the 
acceleration,  respectively. 
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Figure  21:  Position  estimation  error  of  instantaneous  estimates  (high  dynamics  Bierman- 
Thornton  Kalman  filter,  3600  updates). 


acceleration  estimates  of  Figure  24  with  that  of  16,  we  again  see  an  even  greater  difference 
in  performance. 

Now  looking  at  Figures  20  and  24,  we  note  that  the  variations  in  the  acceleration  estimates 
appear  to  be  consistent  with  the  acceleration  noise  used  in  the  Kalman  filter  model,  as 
calculated  at  the  end  of  Section  3.3.  As  further  confirmation  of  this  consistency,  another 
run  of  the  simulation  was  performed  for  the  case  of  3600  updates,  and  the  standard 
deviation  of  the  last  3000  acceleration  estimates,  for  the  x-component  of  acceleration,  was 
computed,  giving  a  result  of  approximately  0.41  m/s2,  which  is  reasonably  close  to  the 
value  of  0.45  m/s2  that  was  calculated  at  the  end  of  Section  3.3.  Based  on  these  results, 
indications  are  that  the  Bierman-Thornton  filter  is  performing  correctly,  and  the  standard 
form  Kalman  filter,  which  was  tested  in  Section  3.3,  is  not.  To  confirm  this,  a  third  form 
of  the  Kalman  filter  (i.e.,  the  Josephson  form),  was  implemented  and  simulations  were  run 
for  comparison  with  the  standard  form  and  Bierman-Thornton  filters.  This  is  described 
in  the  following  section. 


4.4  Josephson  Form  Covariance  Update 

To  confirm  which  of  the  two  high  dynamics  filters  is  giving  the  correct  estimates  of  the 
receiver’s  acceleration,  the  covariance  update  equation  (Equation  36),  as  used  in  the  stan¬ 
dard  filter,  was  replaced  with  the  Josephson  form,  which  is  considered  to  be  less  sensitive 
to  round-off  errors.  The  Josephson  form  of  the  covariance  update  equation  is  as  follows 
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Figure  22:  Bierman-  Thornton  Kalman  filter  position  estimation  error  (3600  updates), 
using  high  dynamics  receiver  model. 


450  - 

400  - 

350  - 

300  - 
aT  250  - 
-S'  200  - 

o 

O 

CD 

>  150  - 

100  - 

50  - 

0 

-50  — 


I 

|  fifrWuVUW'i  nn  ■ * 


0  500  1000  1500  2000  2500 

Update  number 


3000 


3500 


4000 


Figure  23:  Velocity  estimates  of  high  dynamics  Bierman- Thornton  Kalman  filter  (3600 
updates).  The  red,  green  and  blue  plots  are  the  x,  y  and  z  components  of  the 
velocity,  respectively. 
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Figure  24'-  Acceleration  estimates  of  high  dynamics  Bierman-  Thornton  Kalman  filter 
(3600  updates).  The  red,  green  and  blue  plots  are  the  x,  y  and  z  components 
of  the  acceleration,  respectively. 

[2,  p.  294] 

P(k  +  l\k  +  l)  =  [I-W(k+l)H(k  +  l)]P{k  +  l\k)[I-W(k  +  l)H(k  +  l)]T 
+  W  (k  +  1)  R  (k  +  1)  W  (k  +  1)T 

The  Kalman  filter  with  the  high  dynamics  receiver  model,  with  the  Josephson  form  re¬ 
placement,  was  coded  in  Matlab  and  tested  by  simulation.  These  simulations  and  tests 
were  done  with  the  purpose  of  comparing  the  performance  with  that  of  the  standard  form 
Kalman  Filter  (also  using  the  high  dynamics  receiver  model),  and  the  Bierman-Thornton 
version  of  the  Kalman  filter.  All  other  parameters  were  kept  identical  to  those  used  for 
the  standard  form  Kalman  Filter  and  the  Bierman-Thornton  simulations. 

Figures  25,  26,  27  and  28  show  the  results  for  the  case  of  300  updates.  Figure  25  shows  the 
errors  in  the  instantaneous  position  estimates.  Figure  26  shows  the  errors  in  the  Josephson 
Kalman  filter  estimates.  Figures  27  and  28  show  the  velocity  and  acceleration  estimates, 
respectively,  for  the  case  of  300  updates  (same  simulation  as  that  which  produced  Figures 
25  and  26). 

Figures  29,  30,  31  and  32  show  the  results  for  the  case  of  3600  updates.  Figure  29  shows  the 
errors  in  the  instantaneous  position  estimates.  Figure  30  shows  the  errors  in  the  Josephson 
Kalman  filter  estimates.  Figures  31  and  32  show  the  velocity  and  acceleration  estimates, 
respectively,  for  the  case  of  3600  updates  (same  simulation  as  that  which  produced  Figures 
29  and  30). 
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Figure  25:  Position  estimation  error  of  instantaneous  estimates  (high  dynamics  Joseph- 
son  Kalman  filter,  300  updates). 


Figure  26:  Josephson  Kalman  filter  position  estimation  error  (300  updates),  using  high 
dynamics  receiver  model. 
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Figure  21:  Velocity  estimates  of  high  dynamics  Josephson  Kalman  filter  (300  updates). 

The  red,  green  and  blue  plots  are  the  x,  y  and  z  components  of  the  velocity, 
respectively. 
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Figure  28:  Acceleration  estimates  of  high  dynamics  Josephson  Kalman  filter  (300  up¬ 
dates).  The  red,  green  and  blue  plots  are  the  x,  y  and  z  components  of  the 
acceleration,  respectively. 
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Figure  29:  Position  estimation  error  of  instantaneous  estimates  (high  dynamics  Joseph- 
son  Kalman  filter,  3600  updates). 
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Figure  30:  Josephson  Kalman  filter  position  estimation  error  (3600  updates),  using  high 
dynamics  receiver  model. 
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Figure  31:  Velocity  estimates  of  high  dynamics  Josephson  Kalman  filter  (3600  updates). 

The  red,  green  and  blue  plots  are  the  x,  y  and  z  components  of  the  velocity, 
respectively. 


10 

8 

6 


-6 

-8 

10 


0  500  1000  1500  2000  2500  3000  3500  4000 

Update  number 


Figure  32:  Acceleration  estimates  of  high  dynamics  Josephson  Kalman  filter  (3600  up¬ 
dates).  The  red,  green  and  blue  plots  are  the  x,  y  and  z  components  of  the 
acceleration,  respectively. 
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If  we  now  compare  the  simulation  results  for  Josephson  form  filter  with  the  standard  form 
filter  and  the  Bierman-Thornton  UD  filter  we  find  the  following. 

Firstly,  consider  the  simulations  that  were  run  for  300  updates.  Comparing  Figures  10, 
18  and  26,  we  note  that  the  position  estimation  error  appears  to  be  approximately  the 
same  for  all  three  filters.  Comparing  Figures  11,  19  and  27,  we  see  that  the  velocity 
estimates  of  Bierman-Thornton  and  Josephson  filters  are  approximately  the  same.  The 
velocity  estimates  of  standard  form  filter  are  different,  i.e,  they  are  filtered  more  heavily. 
Comparing  Figures  12,  20  and  28,  we  see  that  the  acceleration  estimates  of  Bierman- 
Thornton  and  Josephson  filters  are  approximately  the  same.  The  acceleration  estimates 
of  standard  form  filter  are  considerably  different,  i.e,  they  are  filtered  more  heavily. 

Now  let  us  consider  the  simulations  that  were  run  for  3600  updates.  Comparing  Figures 
14,  22  and  30,  we  see  that  the  position  estimation  error  appears  to  be  approximately 
the  same  for  the  Bierman-Thornton  and  Josephson  filters.  The  position  estimation  error 
for  the  standard  form  filter  appears  to  be  slightly  greater  than  for  the  other  two  filters. 
Comparing  15,  23  and  31,  we  find  that  the  velocity  estimates  of  Bierman-Thornton  and 
Josephson  filters  are  approximately  the  same.  The  velocity  estimates  of  standard  form 
filter  are  different,  i.e.,  they  are  filtered  more  heavily.  Comparing  Figures  16,  24  and 
32,  we  see  that  the  acceleration  estimates  of  Bierman-Thornton  and  Josephson  filters  are 
approximately  the  same.  The  acceleration  estimates  of  standard  form  filter  are  different, 
i.e.,  they  are  filtered  more  heavily. 

In  summary,  the  Bierman-Thornton  and  Josephson  filters  give  approximately  the  same  per¬ 
formance,  whereas  the  standard  form  filter  gives  substantially  different  results,  confirming 
that  the  standard  form  filter  is  performing  incorrectly.  Given  that  the  only  difference 
between  the  standard  form  and  the  Josephson  form  filter  algorithms  is  the  equation  for 
the  state  estimate  covariance  update,  the  result  indicates  that,  for  the  simulations  consid¬ 
ered,  the  primary  cause  of  the  incorrect  behaviour  of  the  standard  form  filter  is  the  effects 
of  numerical  error  on  the  standard  form  state  estimate  covariance  update  equation  i.e., 
Equation  36. 


5  Concluding  Remarks 

In  this  report,  the  mathematics  used  to  convert  GPS  satellite-to-receiver  pseudo-ranges 
to  receiver  position  estimates  is  presented.  First,  the  report  discusses  a  method  that  is 
used  to  determine  instantaneous  estimates  of  receiver  position;  it  then  goes  on  to  develop 
three  Kalman  filter  based  estimators.  The  three  Kalman  filter  estimators  use  a  stationary 
receiver,  low  dynamics,  and  high  dynamics  model  for  the  receiver  kinematics,  respectively. 
The  development  of  the  three  types  of  Kalman  filter,  as  well  as  the  instantaneous  estimator 
is  presented  in  Section  2.  Section  3  then  presents  the  results  of  testing  of  the  filters  by 
simulation.  It  is  found  that  there  are  some  indications  of  degraded  performance  due  to 
numerical  round-off  in  the  case  of  the  high  dynamics  Kalman  filter.  To  investigate  this 
issue  further,  an  alternate  form  of  the  high  dynamics  filter,  using  modified  Cholesky  factors 
of  covariance  matrices,  is  developed  in  Section  4.  The  filter  was  implemented  in  Matlab 
and  tested  by  simulation.  It  is  found  that,  for  the  simulations  considered,  the  alternate 
form  filter  overcomes  the  problems  associated  with  numerical  errors.  The  results  of  the 
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simulations  are  also  in  Section  4. 
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Appendix  A  Matlab  Code  for  Bierman 
Measurement  Update 

The  following  code  was  used  in  the  implementation  of  the  Bierman  UD  measurement 
update  (cf..  Section  4.2). 


function  [KalGain,U_post ,D_post]  =  MyBierman(r,h,U_prior ,D_prior) 

7. 

l  Matlab  implementation  of  the  Bierman  "square  root  filtering  without  square  roots", 
*/.  as  interpreted  from  "Factorization  Methods  for  Discrete  Sequential  Estimation", 

*/.  Gerard  J.  Bierman,  1977,  Section  V.3. 

7. 

70  P.  W.  Sarunic 

7. 

7.  INPUTS: 

7.  r 
7.  h 
7. 

7.  U_prior 
7.  D_prior 
7. 

7.  OUTPUTS: 

7o  U_post 
7o  D_post 
7.  KalGain 
7. 


variance  of  measurement  error 

measurement  sensitivity  (column)  vector  (note  "h"  is  the  same 
as  the  (column)  vector  "a"  in  Bierman’s  book,  Section  V.3). 

unit  upper  triangular  factor  of  covariance  matrix  of  a  priori  state  uncertain 
diagonal  factor  of  covariance  matrix  of  a  priori  state  uncertainty 

upper  triangular  UD  factor  of  a  posteriori  state  uncertainty  covariance 
diagonal  UD  factor  of  a  posteriori  state  uncertainty  covariance 
Kalman  filter  gain 


[VecLength,  Unused]  =  size (U_prior) ; 

f  =  U_prior’  *  h; 

v  =  D_prior  *  f ; 

alpha  =  zeros (VecLength, 1) ; 

alpha (1 , 1)  =  r  +  v(l , 1) *f (1 , 1) ; 

D_post  =  zeros (VecLength, VecLength) ; 

D_post(l,l)  =  D_prior (1 , 1) *r/alpha(l , 1) ; 

K  =  zeros (VecLength, VecLength) ; 

K  (1 , 1)  =  v(l , 1) ; 

lambda  =  zeros (VecLength, 1) ; 

U_post  =  zeros (VecLength, VecLength) ; 

U_post(:,l)  =  U_prior ( : , 1) ;  %  Note:  I  added  this  myself  (it  wasn’t  expicitly 

%  mentioned  in  Bierman’s  pseudo-code). 

for  j  =  2: VecLength 

alpha( j , 1)  =  alpha( j-1 , 1)  +  v(j,l)  *  f(j,l); 

D_post(j , j)  =  D_prior(j , j)  *  alpha(j-l , 1) /alpha(j , 1) ; 
lambda(j,l)  =  -f (j , l)/alpha( j-1 , 1) ; 

U_post(:,j)  =  U_prior(:,j)  +  lambda(j ,1)*K( : , j-1) ; 

K  ( :  ,  j )  =  K(:,j-1)  +  v(j,l)  *  U_prior ( : , j ) ; 
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end; 

KalGain  =  K( : ,VecLength)/alpha(VecLength, 1) ; 
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